/*
 * Author: Honrun
 * brief:  电机驱动
 * Created on 2021年8月17日, 下午2:04
 */

#include "gd32w51x.h"
#include "stdint.h"
#include "stdio.h"
#include "DevicesADC.h"
#include "DevicesSoftTimer.h"
#include "DevicesMotor.h"



MotorInfoType g_typeMotorInfo = {0};

/* 软件定时器 */
SoftTimerTypeDef g_typeSoftTimerMotorUpdateHanle = {0};


void vSoftTimerMotorUpdateCallbackFunction(void *pvHandle)
{
    (void)pvHandle;

    cMotorInfoUpdate();
}



void vMotorInit(void)
{
    vMotorOutputInit();

    vMotorEncodeInit();

    /* 软件定时器每10ms调用一次 */
    cSoftTimerCreateMs(&g_typeSoftTimerMotorUpdateHanle, 10, softTimerOpen | softTimerCircular, vSoftTimerMotorUpdateCallbackFunction);
}

void vMotorOutputInit(void)
{
    timer_oc_parameter_struct timer_ocinitpara = {0};
    timer_parameter_struct timer_initpara = {0};

    /* enable the GPIO clock */
    rcu_periph_clock_enable(MOTOR_PWM_LEFT_MOVE_RCC_GPIO_CLK);
    rcu_periph_clock_enable(MOTOR_PWM_LEFT_BACK_RCC_GPIO_CLK);
    rcu_periph_clock_enable(MOTOR_PWM_RIGHT_MOVE_RCC_GPIO_CLK);
    rcu_periph_clock_enable(MOTOR_PWM_RIGHT_BACK_RCC_GPIO_CLK);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_PWM_LEFT_MOVE_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PWM_LEFT_MOVE_Pin);
    gpio_output_options_set(MOTOR_PWM_LEFT_MOVE_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, MOTOR_PWM_LEFT_MOVE_Pin);
    gpio_af_set(MOTOR_PWM_LEFT_MOVE_GPIO_Port, GPIO_AF_2, MOTOR_PWM_LEFT_MOVE_Pin);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_PWM_LEFT_BACK_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PWM_LEFT_BACK_Pin);
    gpio_output_options_set(MOTOR_PWM_LEFT_BACK_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, MOTOR_PWM_LEFT_BACK_Pin);
    gpio_af_set(MOTOR_PWM_LEFT_BACK_GPIO_Port, GPIO_AF_2, MOTOR_PWM_LEFT_BACK_Pin);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_PWM_RIGHT_MOVE_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PWM_RIGHT_MOVE_Pin);
    gpio_output_options_set(MOTOR_PWM_RIGHT_MOVE_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, MOTOR_PWM_RIGHT_MOVE_Pin);
    gpio_af_set(MOTOR_PWM_RIGHT_MOVE_GPIO_Port, GPIO_AF_2, MOTOR_PWM_RIGHT_MOVE_Pin);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_PWM_RIGHT_BACK_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PWM_RIGHT_BACK_Pin);
    gpio_output_options_set(MOTOR_PWM_RIGHT_BACK_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, MOTOR_PWM_RIGHT_BACK_Pin);
    gpio_af_set(MOTOR_PWM_RIGHT_BACK_GPIO_Port, GPIO_AF_2, MOTOR_PWM_RIGHT_BACK_Pin);


    /* enable the TIMER clock */
    rcu_periph_clock_enable(RCU_TIMER2);
    /* deinit a TIMER */
    timer_deinit(TIMER2);
    /* initialize TIMER init parameter struct */
    timer_struct_para_init(&timer_initpara);
    /* TIMER2 configuration */
    timer_initpara.prescaler         = MOTOR_PWM_PRESCALER - 1;
    timer_initpara.period            = MOTOR_PWM_PERIOD;
    timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
    timer_initpara.counterdirection  = TIMER_COUNTER_UP;
    timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
    timer_initpara.repetitioncounter = 0;
    timer_init(TIMER2, &timer_initpara);


    /* initialize TIMER channel output parameter struct */
    timer_channel_output_struct_para_init(&timer_ocinitpara);
    /* configure TIMER channel output function */
    timer_ocinitpara.outputstate  = TIMER_CCX_ENABLE;
    timer_ocinitpara.outputnstate = TIMER_CCXN_DISABLE;
    timer_ocinitpara.ocpolarity   = TIMER_OC_POLARITY_HIGH;
    timer_ocinitpara.ocnpolarity  = TIMER_OCN_POLARITY_HIGH;
    timer_ocinitpara.ocidlestate  = TIMER_OC_IDLE_STATE_LOW;
    timer_ocinitpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
    timer_channel_output_config(TIMER2, TIMER_CH_0, &timer_ocinitpara);
    timer_channel_output_config(TIMER2, TIMER_CH_1, &timer_ocinitpara);
    timer_channel_output_config(TIMER2, TIMER_CH_2, &timer_ocinitpara);
    timer_channel_output_config(TIMER2, TIMER_CH_3, &timer_ocinitpara);

    /* CH0 configuration in PWM mode0, duty cycle 0% */
    timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 0);
    timer_channel_output_mode_config(TIMER2, TIMER_CH_0, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(TIMER2, TIMER_CH_0, TIMER_OC_SHADOW_DISABLE);

    /* CH1 configuration in PWM mode0, duty cycle 0% */
    timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_1, 0);
    timer_channel_output_mode_config(TIMER2, TIMER_CH_1, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(TIMER2, TIMER_CH_1, TIMER_OC_SHADOW_DISABLE);

    /* CH1 configuration in PWM mode0, duty cycle 0% */
    timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_2, 0);
    timer_channel_output_mode_config(TIMER2, TIMER_CH_2, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(TIMER2, TIMER_CH_2, TIMER_OC_SHADOW_DISABLE);

    /* CH1 configuration in PWM mode0, duty cycle 0% */
    timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_3, 0);
    timer_channel_output_mode_config(TIMER2, TIMER_CH_3, TIMER_OC_MODE_PWM0);
    timer_channel_output_shadow_config(TIMER2, TIMER_CH_3, TIMER_OC_SHADOW_DISABLE);


    /* auto-reload preload enable */
    timer_auto_reload_shadow_enable(TIMER2);
     /* TIMER2 counter enable */
    timer_enable(TIMER2);


    MOTOR_DISABLE_ALL();
}


void vMotorEncodeInit(void)
{
    timer_ic_parameter_struct timer_icinitpara = {0};
    timer_parameter_struct timer_initpara = {0};

    /* enable the GPIO clock */
    rcu_periph_clock_enable(MOTOR_ENCODE_LEFT_RCC_GPIO_CLK);
    rcu_periph_clock_enable(MOTOR_ENCODE_RIGHT_RCC_GPIO_CLK);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_ENCODE_LEFT_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_ENCODE_LEFT_Pin);
    gpio_output_options_set(MOTOR_ENCODE_LEFT_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_166MHZ, MOTOR_ENCODE_LEFT_Pin);
    gpio_af_set(MOTOR_ENCODE_LEFT_GPIO_Port, GPIO_AF_1, MOTOR_ENCODE_LEFT_Pin);

    /*configure TIMERx CHx as alternate function*/
    gpio_mode_set(MOTOR_ENCODE_RIGHT_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_ENCODE_RIGHT_Pin);
    gpio_output_options_set(MOTOR_ENCODE_RIGHT_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_166MHZ, MOTOR_ENCODE_RIGHT_Pin);
    gpio_af_set(MOTOR_ENCODE_RIGHT_GPIO_Port, GPIO_AF_1, MOTOR_ENCODE_RIGHT_Pin);


    rcu_periph_clock_enable(RCU_TIMER0);
    rcu_periph_clock_enable(RCU_TIMER1);

    timer_deinit(TIMER0);
    timer_deinit(TIMER1);
    /* initialize TIMERx init parameter struct */
    timer_struct_para_init(&timer_initpara);
    /* TIMERx configuration */
    timer_initpara.prescaler         = 1 - 1;
    timer_initpara.period            = 65536 - 1;
    timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
    timer_initpara.counterdirection  = TIMER_COUNTER_UP;
    timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
    timer_init(TIMER0, &timer_initpara);
    timer_init(TIMER1, &timer_initpara);

    /* TIMERx CHx input capture configuration */
    timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
    timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
    timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
    timer_icinitpara.icfilter    = 0x01;
    timer_input_capture_config(TIMER0, TIMER_CH_0, &timer_icinitpara);
    timer_input_capture_config(TIMER1, TIMER_CH_0, &timer_icinitpara);

    /* slave mode selection : TIMERx */
    /* TIMERx input trigger : external trigger connected to CI0 */
    timer_input_trigger_source_select(TIMER0, TIMER_SMCFG_TRGSEL_CI0FE0);
    timer_slave_mode_select(TIMER0, TIMER_SLAVE_MODE_EXTERNAL0);

    timer_input_trigger_source_select(TIMER1,TIMER_SMCFG_TRGSEL_CI0FE0);
    timer_slave_mode_select(TIMER1, TIMER_SLAVE_MODE_EXTERNAL0);

    /* enable the TIMERx interrupt */
    timer_interrupt_flag_clear(TIMER0, TIMER_INT_FLAG_UP);
    timer_interrupt_enable(TIMER0, TIMER_INT_UP);

    timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_UP);
    timer_interrupt_enable(TIMER1, TIMER_INT_UP);

    timer_enable(TIMER0);
    timer_enable(TIMER1);
}

/* 需要以固定的频率 MOTOR_ENCODE_UPDATE_FREQUENCY 调用此函数 */
void vMoterUpdateSpeed(void)
{
    int16_t usEncodeNow = 0, usEncodeDelta = 0;

    usEncodeNow = TIMER_CNT(TIMER0);
    usEncodeDelta = usEncodeNow - g_typeMotorInfo.left.encodeLast;
    g_typeMotorInfo.left.encodeLast = usEncodeNow;

    g_typeMotorInfo.left.speedGet = usEncodeDelta * MOTOR_ENCODE_UPDATE_FREQUENCY * g_typeMotorInfo.left.direction;
    g_typeMotorInfo.left.encodeSum += usEncodeDelta * g_typeMotorInfo.left.direction;


    usEncodeNow = TIMER_CNT(TIMER1);
    usEncodeDelta = usEncodeNow - g_typeMotorInfo.right.encodeLast;
    g_typeMotorInfo.right.encodeLast = usEncodeNow;

    g_typeMotorInfo.right.speedGet = usEncodeDelta * MOTOR_ENCODE_UPDATE_FREQUENCY * g_typeMotorInfo.right.direction;
    g_typeMotorInfo.right.encodeSum += usEncodeDelta * g_typeMotorInfo.right.direction;
}

int8_t cMotorSetSpeed(uint8_t ucChannel, float fSpeed)
{
    int32_t iPulseWitch = 0;
    int8_t cError = 0;

    iPulseWitch = fSpeed * 0.01f * MOTOR_PWM_PERIOD;

    /* 当输入端 INA，INB 无输入信号时，请勿将 INA 和 INB 引脚悬空，以避免在高温应用时由于 IC 本体 P/N 结存在微弱漏电流， */
    /* 该漏电流流过下拉电阻会导致悬空引脚的输入电位由低电位变高电位，造成电路输出错误的信号。 */
    if(ucChannel & MOTOR_OUTPUT_CHANNEL_LEFT)
    {
        if(iPulseWitch >= 0)
        {
            TIMER_CH2CV(TIMER2) = 0;
            TIMER_CH0CV(TIMER2) = iPulseWitch;
        }
        else
        {
            TIMER_CH0CV(TIMER2) = 0;
            TIMER_CH2CV(TIMER2) = iPulseWitch;
        }

        g_typeMotorInfo.left.direction = (iPulseWitch >= 0) ? MOTOR_DIRECTION_MOVE : MOTOR_DIRECTION_BACK;
        g_typeMotorInfo.left.speedSet = fSpeed;
    }

    if(ucChannel & MOTOR_OUTPUT_CHANNEL_RIGHT)
    {
        if(iPulseWitch >= 0)
        {
            TIMER_CH3CV(TIMER2) = 0;
            TIMER_CH1CV(TIMER2) = iPulseWitch;
        }
        else
        {
            TIMER_CH1CV(TIMER2) = 0;
            TIMER_CH3CV(TIMER2) = iPulseWitch;
        }

        g_typeMotorInfo.right.direction = (iPulseWitch >= 0) ? MOTOR_DIRECTION_MOVE : MOTOR_DIRECTION_BACK;
        g_typeMotorInfo.right.speedSet = fSpeed;
    }

    return cError;
}

float fMotorGetSpeed(uint8_t ucChannel)
{
    float fSpeed = 0;

    if(ucChannel & MOTOR_OUTPUT_CHANNEL_LEFT)
    {
        fSpeed = g_typeMotorInfo.left.speedGet;
    }
    else if(ucChannel & MOTOR_OUTPUT_CHANNEL_RIGHT)
    {
        fSpeed = g_typeMotorInfo.right.speedGet;
    }

    return fSpeed;
}

float fMotorGetCurrent(uint8_t ucChannel)
{
    float fCurrent = 0.0f;

    fCurrent = fADCGetChannelValue(ucChannel, 16) * 1.0f;

    return fCurrent;
}

int8_t cMotorInfoUpdate(void)
{
    vMoterUpdateSpeed();

    g_typeMotorInfo.left.currentGet = fMotorGetCurrent(ADC_SAMPLE_WHEEL_LEFT_CURRENT);
    g_typeMotorInfo.right.currentGet = fMotorGetCurrent(ADC_SAMPLE_WHEEL_RIGHT_CURRENT);

    return 0;
}




MotorInfoType *ptypeMotorGetInfo(void)
{
    return &g_typeMotorInfo;
}
